#include "rclcpp/rclcpp.hpp"
#include "rms_pkg/msg/heartbeat.hpp"
#include "rms_pkg/srv/robot_status.hpp"
#include "../include/global_logger.h"
#include <chrono>
#include <algorithm>  // for std::find
#include <memory>

using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;

class HeartbeatPublisher : public rclcpp::Node {
public:
    HeartbeatPublisher() : Node("heartbeat_publisher") {
    
        LOG_INFO(" HeartbeatPublisher 初始化开始");

        // 声明参数
        this->declare_parameter<std::string>("robot_id", "robot1001");
        this->declare_parameter<std::string>("robot_type", "inspection");
        this->declare_parameter<std::vector<std::string>>("capabilities", std::vector<std::string>());
        this->declare_parameter<int>("interval", 1);
        this->declare_parameter<float_t>("x",1.0);
        this->declare_parameter<float_t>("y",1.0);
        this->declare_parameter<std::string>("robot_ip", "192.168.1.100");
        this->declare_parameter<std::string>("seer_ip","192.168.1.150");
        this->declare_parameter<std::string>("camera_url","rtsp://admin:Lssec_12@192.168.1.64:554/h265/ch1/main/av_stream");

        // 获取参数
        this->get_parameter("robot_id", robot_id_);
        this->get_parameter("robot_type", robot_type_);
        this->get_parameter("capabilities", capabilities_);
        int interval = 1;
        this->get_parameter("interval", interval);
        if (interval <= 0) {
            LOG_WARN(" 无效的时间间隔，使用默认值 1s");
            interval = 1;
        }
        this->get_parameter("x", x_);
        this->get_parameter("y", y_);
        this->get_parameter("robot_ip", robot_ip_);
        this->get_parameter("seer_ip", seer_ip_);
        this->get_parameter("camera_url", camera_url_);

        // 参数验证（可选）
        std::vector<std::string> valid_types = {"inspection", "harvest", "weed", "grafting", "transport", "guide"};
        if (std::find(valid_types.begin(), valid_types.end(), robot_type_) == valid_types.end()) {
            LOG_WARN(" 无效的 robot_type: {}，使用默认值 'inspection'", robot_type_);
            robot_type_ = "inspection";
        }

        // 创建发布者
        auto qos = rclcpp::QoS(10).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
        publisher_ = this->create_publisher<rms_pkg::msg::Heartbeat>("heartbeat", qos);

        // 构建带命名空间的服务名称
        std::string namespaced_service_name = "/" + robot_id_ + "/get_robot_status";
        client_ = this->create_client<rms_pkg::srv::RobotStatus>(namespaced_service_name);
        LOG_INFO(" 创建服务客户端: {}", namespaced_service_name);
        // 等待服务可用，最多等待5秒
        if (!client_->wait_for_service(5s)) {
            LOG_WARN(" 服务在5秒后仍不可用，将继续执行");
        }

        // 定时器
        timer_ = this->create_wall_timer(
            std::chrono::seconds(interval),
            [this]() {
                this->timer_callback();
            });

        LOG_INFO(" 创建心跳发布者，间隔: {}s", interval);
    }

private:
    void timer_callback() {
        // 立即发送心跳包（使用默认值）
        send_heartbeat_with_defaults();
        
        // 同时发起服务调用以获取最新状态
        auto request = std::make_shared<rms_pkg::srv::RobotStatus::Request>();
        request->query_type = rms_pkg::srv::RobotStatus::Request::QUERY_SEER_STATUS_INFO_TO_HEARTBEAT;
        client_->async_send_request(
            request,
            [this](rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future) {
                this->update_latest_status(future);
            }
        );
    }

    void send_heartbeat_with_defaults() {
        // 发送心跳包，使用默认值或上次获取的值
        auto msg = rms_pkg::msg::Heartbeat();
        msg.robot_id = robot_id_;
        msg.robot_type = robot_type_;
        msg.capabilities = capabilities_;
        msg.x = latest_x_;
        msg.y = latest_y_;
        msg.status = latest_status_;
        msg.current_target = current_target_;
        msg.battery = latest_battery_;
        msg.robot_ip = robot_ip_;       //Jetson控制板IP地址
        msg.seer_ip = seer_ip_;         //Seer控制板IP地址（SRC2000）
        msg.camera_url = camera_url_;

        // 使用 steady_clock 替代 system_clock，避免系统时间调整影响
        msg.timestamp = static_cast<uint64_t>(
            std::chrono::duration_cast<std::chrono::microseconds>(
                std::chrono::steady_clock::now().time_since_epoch()
            ).count()
        );
        
        publisher_->publish(msg);
        
        // 添加更多信息到日志，便于调试
        uint64_t timestamp_ms = msg.timestamp / 1000;
        std::string capabilities_str = "[";
        for (size_t i = 0; i < msg.capabilities.size(); ++i) {
            capabilities_str += msg.capabilities[i];
            if (i < msg.capabilities.size() - 1) {
                capabilities_str += ", ";
            }
        }
        capabilities_str += "]";
        LOG_DEBUG(" 发送心跳包: {} ({}) robot_ip:{} seer_ip:{} capabilities:{} 时间戳: {} ms", 
                      robot_id_, robot_type_, robot_ip_, seer_ip_, capabilities_str, timestamp_ms);
    }

    void update_latest_status(rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedFuture future) {
        try {
            // 处理服务响应
            auto response = future.get();
            LOG_DEBUG(" 更新机器人状态: {} ({}) ", robot_id_, robot_type_);
            if (response->success) {
                // 更新最新的状态值
                latest_x_ = response->x;
                latest_y_ = response->y;
                latest_status_ = response->navigation_state;
                current_target_ = response->current_target;
                latest_battery_ = response->battery;
                
                LOG_DEBUG(" 成功获取机器人状态: 位置({:.2f}, {:.2f}), 电量:{}, 状态:{}",
                              response->x, response->y, response->battery, response->navigation_state);
            } else {
                // 如果服务调用失败，使用默认值或参数值
                latest_x_ = x_;
                latest_y_ = y_;
                latest_status_ = "unknown";
                current_target_ = "unknown";
                latest_battery_ = 0;
                
                LOG_WARN(" 获取机器人状态失败: {}", response->message);
            }
        } catch (const std::exception& e) {
            LOG_ERROR(" 处理服务响应时发生异常: {}", e.what());
            // 出现异常时使用默认值
            latest_x_ = x_;
            latest_y_ = y_;
            latest_status_ = "error";
            latest_battery_ = 0;
        }
    }

    rclcpp::Publisher<rms_pkg::msg::Heartbeat>::SharedPtr publisher_;
    rclcpp::Client<rms_pkg::srv::RobotStatus>::SharedPtr client_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::string robot_id_;
    std::string robot_type_;
    std::vector<std::string> capabilities_;
    std::string robot_ip_;
    std::string seer_ip_;
    std::string camera_url_;
    float_t x_, y_;
    
    // 存储最新的状态信息
    float_t latest_x_;
    float_t latest_y_;
    std::string latest_status_ = "unknown";
    std::string current_target_ = "unknown";
    int8_t latest_battery_ = 0;

};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    // 初始化全局日志记录器（如果尚未初始化）
    GlobalLogger::getInstance().initialize("data/logs", "RMSClient_heartbeat", 5);
    rclcpp::spin(std::make_shared<HeartbeatPublisher>());
    rclcpp::shutdown();
    return 0;
}